/**
 * @file ebex_vector.c
 *
 * @date Sep 14, 2011
 * @author seth
 *
 * @brief This file is part of EBEX State Estimator, created for the EBEX project
 *
 * This software is copyright (C) 2011 Columbia University
 *
 * EBEX State Estimator is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * EBEX State Estimator is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with EBEX State Estimator; if not, write to the Free Software Foundation, Inc.,
 * 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */

#include <math.h>
#include <float.h>

#include <ebex_quaternion.h>
#include <ebex_matrix.h>
#include <ebex_vector.h>

void ebex_3vec_make_compatible_euler(ebex_3vec_t *m_euler, const ebex_3vec_t *m_src)
{
	double dx, dy, dz;

	/* correct differences of about 2PI first */
	dx = m_euler->element[0] - m_src->element[0];
	dy = m_euler->element[1] - m_src->element[1];
	dz = m_euler->element[2] - m_src->element[2];

	while (fabs(dx) > 5.1)
	{
		if (dx > 0.0)
			m_euler->element[0] -= M_2PI;
		else
			m_euler->element[0] += M_2PI;
		dx = m_euler->element[0] - m_src->element[0];
	}
	while (fabs(dy) > 5.1)
	{
		if (dy > 0.0)
			m_euler->element[1] -= M_2PI;
		else
			m_euler->element[1] += M_2PI;
		dy = m_euler->element[1] - m_src->element[1];
	}
	while (fabs(dz) > 5.1)
	{
		if (dz > 0.0)
			m_euler->element[2] -= M_2PI;
		else
			m_euler->element[2] += M_2PI;
		dz = m_euler->element[2] - m_src->element[2];
	}

	if (fabs(dx) > M_PI && fabs(dy) < M_PI_2 && fabs(dz) < M_PI_2)
	{
		if (dx > 0.0)
			m_euler->element[0] -= M_2PI;
		else
			m_euler->element[0] += M_2PI;
	}
	if (fabs(dy) > M_PI && fabs(dz) < M_PI_2 && fabs(dx) < M_PI_2)
	{
		if (dy > 0.0)
			m_euler->element[1] -= M_2PI;
		else
			m_euler->element[1] += M_2PI;
	}
	if (fabs(dz) > M_PI && fabs(dx) < M_PI_2 && fabs(dy) < M_PI_2)
	{
		if (dz > 0.0)
			m_euler->element[2] -= M_2PI;
		else
			m_euler->element[2] += M_2PI;
	}

}

/* returns two euler calculation methods, so we can pick the best */
static void ebex_3vec_2euler_from_mat(ebex_3vec_t *m_eul1, ebex_3vec_t *m_eul2, e_rotation_order m_order, const ebex_mat3x3_t *m_src)
{
	const rotation_order_t *R = GET_ROT_ORDER(m_order);
	int i=R->axis[0];
	int j=R->axis[1];
	int k=R->axis[2];

	double cy = sqrt(
			m_src->col[i].element[i] * m_src->col[i].element[i] + m_src->col[j].element[i] * m_src->col[j].element[i]);

	if (cy > 16.0 * DBL_EPSILON)
	{
		m_eul1->x = atan2(m_src->col[k].element[j], m_src->col[k].element[k]);
		m_eul1->y = atan2(-m_src->col[k].element[i], cy);
		m_eul1->z = atan2(m_src->col[j].element[i], m_src->col[i].element[i]);

		m_eul2->x = atan2(-m_src->col[k].element[j], -m_src->col[k].element[k]);
		m_eul2->y = atan2(-m_src->col[k].element[i], -cy);
		m_eul2->z = atan2(-m_src->col[j].element[i], -m_src->col[i].element[i]);
	}
	else
	{
		m_eul1->x = atan2(-m_src->col[j].element[k], m_src->col[j].element[j]);
		m_eul1->y = atan2(-m_src->col[k].element[i], cy);
		m_eul1->z = 0;

		ebex_3vec_copy(m_eul2, m_eul1);
	}

	if (R->parity)
	{
		ebex_3vec_neg(m_eul1, m_eul1);
		ebex_3vec_neg(m_eul2, m_eul2);
	}
}


void ebex_3vec_euler_from_3mat_ref(ebex_3vec_t *m_dest, ebex_3vec_t *m_ref, e_rotation_order m_order, const ebex_mat3x3_t *m_src)
{
	ebex_3vec_t eul1;
	ebex_3vec_t eul2;
	double diff1, diff2;

	ebex_3vec_2euler_from_mat(&eul1, &eul2, m_order, m_src);

	ebex_3vec_make_compatible_euler(&eul1, m_ref);
	ebex_3vec_make_compatible_euler(&eul2, m_ref);

	diff1= fabs(eul1.x-m_ref->x) + fabs(eul1.y-m_ref->y) + fabs(eul1.z-m_ref->z);
	diff2= fabs(eul2.x-m_ref->x) + fabs(eul2.y-m_ref->y) + fabs(eul2.z-m_ref->z);

	/* return best, which is just the one with lowest difference */
	if (diff1 > diff2)
		ebex_3vec_copy(m_dest, &eul2);
	else
		ebex_3vec_copy(m_dest, &eul1);
}

void ebex_3vec_euler_from_3mat(ebex_3vec_t *m_dest, e_rotation_order m_order, const ebex_mat3x3_t *m_src)
{
	ebex_3vec_t eul1;
	ebex_3vec_t eul2;

	ebex_3vec_2euler_from_mat(&eul1, &eul2, m_order, m_src);

	/* return best, which is just the one with lowest difference */
	if (fabs(eul1.x) + fabs(eul1.y) + fabs(eul1.z) > fabs(eul2.x) + fabs(eul2.y) + fabs(eul2.z) )
		ebex_3vec_copy(m_dest, &eul2);
	else
		ebex_3vec_copy(m_dest, &eul1);
}

void ebex_3vec_euler_from_quat(ebex_3vec_t *m_dest, e_rotation_order m_order,  const ebex_quat_t *m_src)
{
	ebex_mat3x3_t tmp_mat;

	ebex_matrix_from_quat(&tmp_mat, m_src);
	ebex_3vec_euler_from_3mat(m_dest, m_order, &tmp_mat);

}
